using System;
using System.Threading;
using LegoBotPrimaAbstractions;
using Microsoft.Ccr.Core;
using Microsoft.Robotics.Services.Drive.Proxy;
using W3C.Soap;

namespace LegoBotPrima
{
    public class LegoBotPrimaWorkflow : ILegoBotPrimaWorkflow
    {
        public const double Speed_Fast = 1.0;
        public const double Speed_Medium = 0.5;
        public const double Speed_Slow = 0.15;
        public const double Speed_None = 0;

        public static double Forward_Avg = 20.75;
        public static double Forward_Var = 2.6875;

        private RunMode _mode = RunMode.Stop;

        private int _tickNumber;
        public System.Timers.Timer _timer;

        public LegoBotPrimaWorkflow(ILegoBotPrimaService service, IWorkflowLogger callLogger, IBotActionController controller)
        {
            Service = service;
            Logger = callLogger;
            ActionController = controller;
            ActionController.Initialize(this, Logger);

            this._timer = new System.Timers.Timer(100);
            this._timer.Elapsed += Timer_Elapsed;
            this._timer.Start();
        }

        
        private void Timer_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
        {
            if(this.CurrentAction != null)
            {
                this.CurrentAction.OnTimerTick(Interlocked.Increment(ref _tickNumber));
            }
        }

        public void Dispose()
        {
            this._timer.Stop();
        }

        protected IWorkflowLogger Logger { get; private set; }
        protected IBotAction CurrentAction { get; private set; }
        protected IBotActionController ActionController { get; private set; }

        public ILegoBotPrimaService Service { get; private set; }
        public IMainPrimaWindow UI { get; set; }
        public double RotationAngle { get; private set; }
        public DateTime RotationAngleTimeStamp { get; private set; }
        public RunMode Mode { get { return this._mode; } }

        public void SetAction(IBotAction action)
        {
            Logger.Call(log => log.LogMethodCall("SetAction", action));
            if (action == null) return;
            var previous = this.CurrentAction;
            action.PreviousAction = previous;
            action.Workflow = this;
            action.Logger = Logger;
            action.MarkStart();
            action.Initialize();
            if (previous != null)
            {
                previous.MarkStop();
                previous.Dispose();
            }
            this.CurrentAction = ActionController.OnSetAction(previous, action);

            if (UI != null)
                UI.SetBotActionType(action);

            this._mode = ActionController.OnSetActionChangeMode(this._mode, previous, action);
        }

        public void Shutdown()
        {
            Logger.Call(log => log.LogMethodCall("Shutdown"));
            Logger.Dispose();
        }

        public void Stop()
        {
            Logger.Call(log => log.LogMethodCall("Stop"));
            Service.SetDrivePower(Speed_None, Speed_None);
        }
        public void GoLeft()
        {
            Logger.Call(log => log.LogMethodCall("Call"));
            Service.SetDrivePower(Speed_Medium, -Speed_Medium);
        }
        public void GoLeftSlow()
        {
            Logger.Call(log => log.LogMethodCall("GoLeftSlow"));
            Service.SetDrivePower(Speed_Slow, -Speed_Slow);
        }
        public void GoLeftFast()
        {
            Logger.Call(log => log.LogMethodCall("GoLeftFast"));
            Service.SetDrivePower(Speed_Fast, -Speed_Fast);
        }
        public void GoRight()
        {
            Logger.Call(log => log.LogMethodCall("GoRight"));
            Service.SetDrivePower(-Speed_Medium, Speed_Medium);
        }
        public void GoRightSlow()
        {
            Logger.Call(log => log.LogMethodCall("GoRightSlow"));
            Service.SetDrivePower(-Speed_Slow, Speed_Slow);
        }
        public void GoRightFast()
        {
            Logger.Call(log => log.LogMethodCall("GoRightFast"));
            Service.SetDrivePower(-Speed_Fast, Speed_Fast);
        }
        public void GoForward()
        {
            Logger.Call(log => log.LogMethodCall("GoForward"));
            Service.SetDrivePower(Speed_Medium, Speed_Medium);
        }
        public void GoForwardSlow()
        {
            Logger.Call(log => log.LogMethodCall("GoForwardSlow"));
            Service.SetDrivePower(Speed_Slow, Speed_Slow);
        }
        public void GoForwardFast()
        {
            Logger.Call(log => log.LogMethodCall("GoForwardFast"));
            Service.SetDrivePower(Speed_Fast, Speed_Fast);
        }
        public void GoBackward()
        {
            Logger.Call(log => log.LogMethodCall("GoBackward"));
            Service.SetDrivePower(-Speed_Medium, -Speed_Medium);
        }
        public void GoBackwardSlow()
        {
            Logger.Call(log => log.LogMethodCall("GoBackwardSlow"));
            Service.SetDrivePower(-Speed_Slow, -Speed_Slow);
        }
        public void GoBackwardFast()
        {
            Logger.Call(log => log.LogMethodCall("GoBackwardFast"));
            Service.SetDrivePower(-Speed_Fast, -Speed_Fast);
        }
        public void OnCompassChanged(double degrees, DateTime timeStamp)
        {
            Logger.Call(log => log.LogMethodCall("OnCompassChanged", degrees, timeStamp));

            this.RotationAngle = degrees;
            this.RotationAngleTimeStamp = timeStamp;
            
            this.CurrentAction.OnCompassChanged(degrees, timeStamp);
            UI.SetCompass(degrees);

            degrees += Config.Instance.InitialAngle;
            degrees = degrees % 360;
            UI.DrawBotAngleOnMap(degrees);
        }
        public void OnBumperChanged(bool pressed, DateTime timeStamp, string bumperName)
        {
            Logger.Call(log => log.LogMethodCall("OnBumperChanged", pressed, timeStamp, bumperName));
            this.CurrentAction.OnBumperChanged(pressed, timeStamp, bumperName);
            UI.SetBumper(bumperName, pressed);
        }
        public void OnDistanceChanged(int distance, DateTime timeStamp)
        {
            Logger.Call(log => log.LogMethodCall("OnDistanceChanged", distance, timeStamp));
            this.CurrentAction.OnDistanceChanged(distance, timeStamp);
            this.DrawDistanceOnMap(distance);
            UI.SetUltrasonic(distance);
        }
        public void OnAccelerometerChanged(double x, double y, double z, DateTime timeStamp)
        {
            Logger.Call(log => log.LogMethodCall("OnAccelerometerChanged", x, y, z, timeStamp));
            this.CurrentAction.OnAccelerometerChanged(x, y, z, timeStamp);
        }

        public void DrawBotAngleOnMap(double rotationAngle)
        {
            Logger.Call(log => log.LogMethodCall("DrawBotAngleOnMap", rotationAngle));

            rotationAngle += Config.Instance.InitialAngle;
            rotationAngle = rotationAngle % 360;
            UI.DrawBotAngleOnMap(rotationAngle);
        }
        public void DrawBotPositionChangeOnMap(double xChange, double yChange)
        {
            Logger.Call(log => log.LogMethodCall("DrawBotPositionChangeOnMap", xChange, yChange));

            UI.DrawBotPositionChangeOnMap(xChange, yChange);
        }
        public void DrawDistanceOnMap(int distance)
        {
            Logger.Call(log => log.LogMethodCall("DrawDistanceOnMap", distance));

            UI.DrawDistanceOnMap(distance);
        }
        public void EndAction(IBotAction action)
        {
            Logger.Call(log => log.LogMethodCall("EndAction", action));
            
            ActionController.OnEndAction(action);
        }

        public void Error(string s, Fault failt)
        {
            if (failt != null) s += " Reason:" + failt.Reason;
            Logger.LogKeyState("ERROR:"+s, failt);

            UI.Error(s);
        }
        public void GetDrivePortState(Handler<DriveDifferentialTwoWheelState> success, Handler<Fault> error)
        {
            Logger.Call(log => log.LogMethodCall("GetDrivePortState", success, error));

            Service.GetDrivePortState(success, error);
        }
        

    }
}